/*==============================================================================
  4-motor control via the motor shield.

  Soft-start/stop ramping (not just instant full-speed) reduces current
  inrush on the shared solar/battery bus
==============================================================================*/
#ifndef DRIVETRAIN_H
#define DRIVETRAIN_H

#include <Arduino.h>
#include <Adafruit_MotorShield.h>
#include "Config.h"

enum class ManeuverState : uint8_t { IDLE, RAMP_UP, RUNNING, RAMP_DOWN, BACKING_UP, TURNING };

class Drivetrain {
public:
  void begin(Adafruit_MotorShield &shield);
  void update(unsigned long nowMs);

  bool isBusy() const { return _state != ManeuverState::IDLE; }
  void startForwardHop(unsigned long runDurationMs, float speedScale);
  void startAvoidanceManeuver(float speedScale);
  void emergencyStop();

  int headingSign() const { return _heading; }

private:
  Adafruit_DCMotor *_flMotor = nullptr;
  Adafruit_DCMotor *_frMotor = nullptr;
  Adafruit_DCMotor *_blMotor = nullptr;
  Adafruit_DCMotor *_brMotor = nullptr;

  ManeuverState _state = ManeuverState::IDLE;
  unsigned long _stateStartMs = 0;
  unsigned long _plannedRunMs = 0;
  unsigned long _turnDurationMs = 0;
  int _targetSpeed = 0;
  int _heading = 1; // flips after each avoidance turn

  void applyForward(int speed);
  void applyBackward(int speed);
  void applyTurn(int speed);
  void stopAllMotors();
};

#endif
